

import serial

import codecs
import rclpy

from imu_driver.driver import Ros2IMUDriver


def main(args=None):
    rclpy.init(args=args)

    driver = Ros2IMUDriver()

    serial_port = driver.declare_parameter('port', '/dev/ttyS3').value
    serial_baud = driver.declare_parameter('baud', 115200).value

    try:
        IMU = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
        if IMU.isOpen():
            print("open success1111")
        else:
            print("open failed")
        driver.get_logger().info("Successfully connected to {0} at {1}.".format(serial_port, serial_baud))
        try:
            while rclpy.ok():
                count = IMU.inWaiting()
                if count > 0:
                    IMU.write(codecs.decode('59534d12005001b06a','hex'))
                    data = IMU.read(83) # 5+ 14+14+14+18+4+6+6  +2
                    if driver.Read_IMUdata(data):
                        pass
                    else:
                        IMU.close()
                        IMU = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
                        if IMU.isOpen():
                            print("open success")
                        else:
                            print("open failed")
        except Exception as e:
            driver.get_logger().error("Ros error: {0}".format(e))
            IMU.close()  # Close GPS serial port
    except serial.SerialException as ex:
        driver.get_logger().fatal("Could not open serial port: I/O error({0}): {1}".format(ex.errno, ex.strerror))
